/****************************************************************************
 *
 *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <poll.h>
#include <uORB/topics/manual_control_setpoint.h>

#define YT_PITCH    (0)
#define YT_ROLL     (1)
#define YT_YAW      (2)

static bool thread_should_exit = false;		/**< Daemon exit flag */
static bool thread_running = false;		/**< Daemon status flag */
static int daemon_task;				/**< Handle of daemon task / thread */
uint16_t do_crc(unsigned char * message, int len);
int yt_stick_control_thread_main(int argc, char *argv[]);
__EXPORT int yt_stick_control_main(int argc, char *argv[]);

int OpenSerial(const char *dev)
{
	// 波特率为115200
	int rate = B115200;

	// open uart
	int fd = open(dev, O_RDWR | O_NOCTTY);
	int termios_state = -1;

	if (fd < 0) {
		PX4_ERR("failed to open uart device!");
		return -1;
	}

	// set baud rate
	struct termios config;
	tcgetattr(fd, &config);


	config.c_cflag &= ~CSIZE;
	config.c_cflag |= CS8;         // 8数据位
	config.c_cflag &= ~PARENB;     // 无奇偶校验
	config.c_cflag &= ~CSTOPB;     // 1停止位
	config.c_cflag &= ~CRTSCTS;    // 关闭硬件流

	/* Set baud rate */
	if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) {
		PX4_WARN("ERR SET BAUD %s: %d\n", dev, termios_state);
		close(fd);
		return -1;
	}

	if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) {
		PX4_WARN("ERR SET CONF %s\n", dev);
		close(fd);
		return -1;
	}


	return fd;
}

int SendBytes(int fd,const void *data, size_t len)
{
	for (size_t i = 0; i < len; i++) {
		// Block until we can send a byte
		write(fd, ((const uint8_t *)data) + i, 1);
		usleep(1);
	}

	return 0;
}
//angle范围：1100 ，1900
//aux_raw:-1,1
void ChangeAngle(int fd,uint8_t which, float aux_raw)
{
	static uint8_t buf[7];
	uint16_t tmp;
	uint8_t *s;
	uint8_t num=7;
	buf[0]=0xFA;
	buf[1]=0x02;
	if(which==YT_PITCH){
		buf[2]=0x0B;
	}else if(which==YT_ROLL){
		buf[2]=0x0A;
	}else{
		buf[2]=0x0C;
	}
	uint16_t angle = 1500 + aux_raw * 400;
	buf[3]=(uint8_t)(angle);
	buf[4]=(uint8_t)((angle)>>8);
	tmp=do_crc(buf,5);
	buf[5]=(uint8_t)tmp;
	buf[6]=(uint8_t)(tmp>>8);

	s = (uint8_t *) buf;
	SendBytes(fd,s,num);
}

uint16_t do_crc(unsigned char * message, int len)
{
	int i, j;
	unsigned short crc_reg;

	crc_reg = (message[0] << 8) + message[1];
	for (i = 0; i < len; i++)
	{
		if (i < len - 2)
		for (j = 0; j <= 7; j++)
		{
			if ((short)crc_reg < 0)
			crc_reg = ((crc_reg << 1) + (message[i + 2] >> (7 - i))) ^ 0x1021;
			else
			crc_reg = (crc_reg << 1) + (message[i + 2] >> (7 - i));
		}
		else
		for (j = 0; j <= 7; j++)
		{
			if ((short)crc_reg < 0)
			crc_reg = (crc_reg << 1) ^ 0x1021;
			else
			crc_reg <<= 1;
		}
	}
	return crc_reg;
}






int yt_stick_control_main(int argc, char *argv[])
{

	if (!strcmp(argv[1], "start")) {
		if (thread_running) {
			PX4_ERR("already running\n");
			/* this is not an error */
			exit(0);
		}

		thread_should_exit = false;
		daemon_task = px4_task_spawn_cmd("yt_stick_control",
						 SCHED_DEFAULT,
						 SCHED_PRIORITY_MAX - 5,
						 2000,
						 yt_stick_control_thread_main,
						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
		exit(0);
	}

	if (!strcmp(argv[1], "stop")) {
		thread_should_exit = true;
		exit(0);
	}

	if (!strcmp(argv[1], "status")) {
		if (thread_running) {
			PX4_ERR("running");

		} else {
			PX4_ERR("stopped");
		}

		exit(0);
	}


	exit(1);
}

int yt_stick_control_thread_main(int argc, char *argv[])
{
	if(argc < 2){
		PX4_ERR("params need 2");
		return -1;
	}
	const char *uart_name = argv[1];

	PX4_INFO("opening port %s", uart_name);

	int fd = OpenSerial(uart_name);

	if(fd < 0){
		return -1;
	}

	/* subscribe to vehicle_acceleration topic */
	int mci_fd = orb_subscribe(ORB_ID(manual_control_input));

	/* limit the update rate to 10 Hz */
	orb_set_interval(mci_fd, 100);


	px4_pollfd_struct_t fds[] = {
		{ .fd = mci_fd,   .events = POLLIN },
	};

	int error_counter = 0;

	thread_running = true;

	while (!thread_should_exit) {


		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
		int poll_ret = px4_poll(fds, 1, 1000);

		/* handle the poll result */
		if (poll_ret == 0) {
			/* this means none of our providers is giving us data */
			PX4_ERR("Got no data within a second");

		} else if (poll_ret < 0) {
			/* this is seriously bad - should be an emergency */
			if (error_counter < 10 || error_counter % 50 == 0) {
				/* use a counter to prevent flooding (and slowing us down) */
				PX4_ERR("ERROR return value from poll(): %d", poll_ret);
			}

			error_counter++;

		} else {

			if (fds[0].revents & POLLIN) {
				/* obtained data for the first file descriptor */
				struct manual_control_setpoint_s manual_control_input;
				/* copy sensors raw data into local buffer */
				orb_copy(ORB_ID(manual_control_input), mci_fd, &manual_control_input);
				ChangeAngle(fd,YT_PITCH,manual_control_input.aux1);
				ChangeAngle(fd,YT_YAW,manual_control_input.aux2);
			}


		}

	}

	PX4_INFO("exiting");
	thread_running = false;

	fflush(stdout);
	return 0;
}
